﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Input;
using System.Xml;

using Microsoft.Research.Kinect.Nui;


namespace KinectMsSdkServer
{
    class KinectHandler
    {
        private Runtime nui;
        private ClientConnectionHandler cch;

        public KinectHandler()
        {
        }

        void nui_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            if (cch != null)
            {
                SkeletonFrame skeletonFrame = e.SkeletonFrame;

                Vector normalToGravity = skeletonFrame.NormalToGravity;

                XmlDocument doc = new XmlDocument();

                XmlNode xmlnode = doc.CreateNode(XmlNodeType.XmlDeclaration, "", "");
                doc.AppendChild(xmlnode);

                XmlNode root = doc.CreateElement("Skeleton");
                doc.AppendChild(root);

                XmlNode frameNode = doc.CreateElement("frameNumber");
                frameNode.InnerText = skeletonFrame.FrameNumber.ToString();
                root.AppendChild(frameNode);

                XmlNode timeStampNode = doc.CreateElement("timeStamp");
                timeStampNode.InnerText = skeletonFrame.TimeStamp.ToString();
                root.AppendChild(timeStampNode);


                foreach (SkeletonData data in skeletonFrame.Skeletons)
                {

                    if (data.TrackingState == SkeletonTrackingState.Tracked)
                    {
                        XmlNode skeletonData = doc.CreateElement("skeletonData");
                        root.AppendChild(skeletonData);

                        XmlNode trackingId = doc.CreateElement("trackingId");
                        trackingId.InnerText = data.TrackingID.ToString();
                        skeletonData.AppendChild(trackingId);

                        XmlNode userIndex = doc.CreateElement("userIndex");
                        userIndex.InnerText = data.UserIndex.ToString();
                        skeletonData.AppendChild(userIndex);

                        JointsCollection joints = data.Joints;

                        foreach (Joint j in joints)
                        {
                            XmlNode joint = doc.CreateElement("joint");

                            XmlNode jPositionX = doc.CreateElement("positionX");
                            jPositionX.InnerText = j.Position.X.ToString();
                            joint.AppendChild(jPositionX);

                            XmlNode jPositionY = doc.CreateElement("positionY");
                            jPositionY.InnerText = j.Position.Y.ToString();
                            joint.AppendChild(jPositionY);

                            XmlNode jPositionZ = doc.CreateElement("positionZ");
                            jPositionZ.InnerText = j.Position.Z.ToString();
                            joint.AppendChild(jPositionZ);

                            XmlNode jointID = doc.CreateElement("jointId");
                            jointID.InnerText = j.ID.ToString();
                            joint.AppendChild(jointID);
                        }

                        XmlNode positionX = doc.CreateElement("positionX");
                        positionX.InnerText = data.Position.X.ToString();
                        skeletonData.AppendChild(positionX);

                        XmlNode positionY = doc.CreateElement("positionY");
                        positionY.InnerText = data.Position.Y.ToString();
                        skeletonData.AppendChild(positionY);

                        XmlNode positionZ = doc.CreateElement("positionZ");
                        positionZ.InnerText = data.Position.Z.ToString();
                        skeletonData.AppendChild(positionZ);

                    }
                }


                cch.sendSkeleton(doc.OuterXml);
            }
     
        }

        internal void startSkeletonTracking(ClientConnectionHandler clientConnectionHandler)
        {
            cch = clientConnectionHandler;

        }

        public void setUpAndRun() 
        {
            nui = new Runtime();

            try
            {
                nui.Initialize(RuntimeOptions.UseDepthAndPlayerIndex | RuntimeOptions.UseSkeletalTracking);

            }
            catch (InvalidOperationException)
            {
                System.Console.WriteLine("Runtime initialization failed. Please make sure Kinect device is plugged in.");
                return;
            }

            try
            {
                nui.DepthStream.Open(ImageStreamType.Depth, 2, ImageResolution.Resolution320x240, ImageType.DepthAndPlayerIndex);

            }
            catch (InvalidOperationException e)
            {
                System.Console.WriteLine("Failed to open stream. Please make sure to specify a supported image type and resolution." + e.Message);
                return;
            }
            nui.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(nui_SkeletonFrameReady);
            
        }

        internal void testSkeletonTracking(ClientConnectionHandler clientConnectionHandler)
        {
              if (cch != null)
            {
       
                XmlDocument doc = new XmlDocument();

                XmlNode xmlnode = doc.CreateNode(XmlNodeType.XmlDeclaration, "", "");
                doc.AppendChild(xmlnode);

                XmlNode root = doc.CreateElement("Skeleton");
                doc.AppendChild(root);

                XmlNode frameNode = doc.CreateElement("frameNumber");
                frameNode.InnerText = "frameNumber";
                root.AppendChild(frameNode);

                XmlNode timeStampNode = doc.CreateElement("timeStamp");
                timeStampNode.InnerText = "timeStamp";
                root.AppendChild(timeStampNode);


                for (int i=0; i<2; i++)
                {

                    if (true)
                    {
                        XmlNode skeletonData = doc.CreateElement("skeletonData");
                        root.AppendChild(skeletonData);

                        XmlNode trackingId = doc.CreateElement("trackingId");
                        trackingId.InnerText = "trackingId"+i;
                        skeletonData.AppendChild(trackingId);

                        XmlNode userIndex = doc.CreateElement("userIndex");
                        userIndex.InnerText = "userIndex"+i;
                        skeletonData.AppendChild(userIndex);

                        

                        for (int j=0; j<2;j++)
                        {
                            XmlNode joint = doc.CreateElement("joint");

                            XmlNode jPositionX = doc.CreateElement("positionX");
                            jPositionX.InnerText = "positionX"+i+j;
                            joint.AppendChild(jPositionX);

                            XmlNode jPositionY = doc.CreateElement("positionY");
                            jPositionY.InnerText = "positionY"+i+j;
                            joint.AppendChild(jPositionY);

                            XmlNode jPositionZ = doc.CreateElement("positionZ");
                            jPositionZ.InnerText = "positionZ"+i+j;
                            joint.AppendChild(jPositionZ);

                            XmlNode jointID = doc.CreateElement("jointId");
                            jointID.InnerText = "jointId " + i + j;
                            joint.AppendChild(jointID);
                        }

                        XmlNode positionX = doc.CreateElement("positionX");
                        positionX.InnerText = "positionX"+i;
                        skeletonData.AppendChild(positionX);

                        XmlNode positionY = doc.CreateElement("positionY");
                        positionY.InnerText = "positionY"+i;
                        skeletonData.AppendChild(positionY);

                        XmlNode positionZ = doc.CreateElement("positionZ");
                        positionZ.InnerText = "positionZ"+i;
                        skeletonData.AppendChild(positionZ);

                    }
                }


                cch.sendSkeleton("SKELETON: " + doc.OuterXml);
            }
        }
    }

}
